/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Test for HubCtrl protocol
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#define _GNU_SOURCE
#include <string.h>
#include "protocolDispatcher.h"
#include "protocol/hubctrlHdr.h"
#include "owndebug.h"
#include "ownutils.h"
#include "apsecurity.h"
#include <signal.h>
#include <curses.h>
#include <assert.h>
#include <time.h>

//*****************************************************************************
//*****************************************************************************
#define GIMNETAP_HUBCTRLPROTOCOL_TESTAPP_VERSIONSTRING "v0.0-Alpha"
//*****************************************************************************
//*****************************************************************************

static volatile int run = 1;
static int verbose = ODINFO;
static ownThreadHandle signalHandlerThread = NULL;
static ownThreadHandle protocolDispProcessorThreadHandle = NULL;
static ownMutexHandle lock;
static ownCondHandle cond;
static WINDOW *w = NULL;
//*****************************************************************************
static uint32_t requested_hubId = 0x00020000;
static char secret_key[1024] = "none";
static unsigned int secret_key_size = 4;
static uint32_t targetNodeId = 0x00000001;
static uint32_t localNodeId = 0x00000000;
static uint32_t messageSeq = 0;
static uint32_t lastReceivedMessageSeq_async = 0;
static TProtocolDisp aplink;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

//*****************************************************************************

static void Lock()
{
  ownMutex_Lock(lock);
}
//*****************************************************************************

static void Unlock()
{
  ownMutex_Unlock(lock);
}
//*****************************************************************************

static void notifyReply(const int id)
{
  Lock();
  lastReceivedMessageSeq_async = id;
  ownCond_Broadcast(cond);
  Unlock();
}
//*****************************************************************************

static int waitReply(const int id, const int timeout)
{
  int result = 0;
  ownTime_ms_t begin = ownTime_get_ms();
  ownTime_ms_delta_t left = timeout;

  Lock();
  while(lastReceivedMessageSeq_async < id && left > 0) {
    ownCond_Wait(cond, lock, left);
    left = ownTime_get_ms_left(timeout, begin);
  }
  
  result = (lastReceivedMessageSeq_async >= id);
  Unlock();
  
  return result;
}
//*****************************************************************************

static int authenticateMessage(const uint8_t *message,
                               const unsigned int messagelen,
                               const THubCtrlSubHeaderAuthentication *auth,
                               const uint8_t *key,
                               const unsigned int keylen,
                               const unsigned int validity_time_s)
{
  // Allocate!
  uint8_t msg_sha1[sizeof(auth->sha1)];
  uint8_t calc_sha1[sizeof(auth->sha1)];
  unsigned int calc_sha1_size = sizeof(auth->sha1);
  int result = 1;

  // Check timestamp validity
  const time_t now = time(NULL);
  if ((now - validity_time_s) < auth->timestamp_s &&
      (now + validity_time_s) > auth->timestamp_s) {
    
    // Copy!
    memcpy(msg_sha1, auth->sha1, sizeof(auth->sha1));
    
    // Zero! (typecasting required to override constness)
    memset((void*)auth->sha1, 0x00, sizeof(auth->sha1));
    
    // Calculate!
    apsecurityCalculateHMAC(calc_sha1,
                            &calc_sha1_size,
                            (void*)key,
                            keylen,
                            message,
                            messagelen);
    
    // Write back!
    memcpy((void*)auth->sha1, msg_sha1, sizeof(auth->sha1));

    // Compare!
    result = memcmp(msg_sha1, calc_sha1, sizeof(auth->sha1));

    // Modify to match specs! (If not Zero == Change to failure)
    if (result) result = -1;

  }
  
  // return!
  return result;
}
//*****************************************************************************

static int protocolDispProcessorThread(void *p)
{
  TProtocolDisp *link = (TProtocolDisp *)p;
  while(run) {
    if (protocolDispProcess(link, 1000)) {
      dPrint(ODTEST,"Process returned error!");
      run = 0;
    }
  }
  return 0;
}
//*****************************************************************************

static void hubctrlProtocolCallback(int protocol_number,
                                    const unsigned char *data,
                                    int size,
                                    void *userPtr)
{
  // This function gets called when a HubCtrlProtocol packet is received
  // from Hub.
  const THubCtrlProtocolHeader *frame = (const THubCtrlProtocolHeader *)data;

  if (size >= sizeof(THubCtrlProtocolHeader) &&
      size == frame->protocol_id.total_size) {
    if (frame->flags & KHubCtrlFlagReply) {

      // Protocol size match; We have enough data to process call
      if (frame->request == KHubCtrlRequestQueryLocalNodeId &&
          size >= sizeof(THubCtrlProtocolHeader)+sizeof(THubCtrlSubHeaderNodeInfo)) {
        const THubCtrlSubHeaderNodeInfo *ni = (void*)(frame+1);
      
        printw("\rOwn address for this module is '0x%08x' (Link delay %u us)\n",
               ni->nodeid, ni->link_delay_us);
        localNodeId = ni->nodeid;
      } else if (frame->request == KHubCtrlRequestHubVersion &&
                 size >= sizeof(THubCtrlProtocolHeader)+sizeof(THubCtrlSubHeaderVersion)) {
        const THubCtrlSubHeaderVersion *vi = (void*)(frame+1);
        printw("\rRemote Hub version: %d.%d.%d.%d (%d) (Protocol %d)\n",
               vi->major,
               vi->minor,
               vi->sub,
               vi->fix,
               vi->build,
               vi->protocol_ver);
             
      } else if (frame->request == KHubCtrlRequestRegisterAP) {
        // RegisterAP received. Introduce a few variables to make
        // code more readable (Compiler should optimize these out)
        const unsigned int bsize = sizeof(THubCtrlProtocolHeader);
        const unsigned int rsize = sizeof(THubCtrlSubHeaderRegisterAP);
        const unsigned int asize = sizeof(THubCtrlSubHeaderAuthentication);
      
        printw("\rReceived a reply for RegisterAP Id=%u, operation %s\n",
               frame->message_id,
               frame->result==KHubCtrlResultOK ? "was a success!" :
               frame->result==KHubCtrlResultIdReserved ? "failed due to reserved ID." :
               frame->result==KHubCtrlResultAuthExpired ? "failed due to expired authorization." :
               "produced an unknown error!");
      
        if (size == bsize+rsize+asize) {
          const THubCtrlSubHeaderRegisterAPService *rap = (void*)(data+bsize); 
          const THubCtrlSubHeaderAuthentication *auth = (void*)(data+bsize+rsize); 
        
          printw("RegisterAP Reply Id=%u contained request for NodeID: 0x%08x\n",
                 frame->message_id, rap->requested_id);
        
          const int a = authenticateMessage(data, size, auth, (void*)secret_key, secret_key_size, 10);
          if (a == 0) {
            printw("RegisterAP Reply Id=%u was succesfully Authenticated\n",frame->message_id);
 
          } else if (a == 1) {
            printw("RegisterAP Reply Id=%u, received reply had expired!\n", frame->message_id);

          } else {
            printw("RegisterAP Reply Id=%u failed to Authenticate!!!\n", frame->message_id);

          }



        } else if (size == bsize+rsize) {
          const THubCtrlSubHeaderRegisterAP *rap = (void*)(data+bsize); 
          printw("RegisterAP Reply Id=%u contained request for NodeID: 0x%08x\n",
                 frame->message_id, rap->peer_id);

        } else if (size == bsize) {
          printw("RegisterAP Reply Id=%u contained no additional headers.\n",

                 frame->message_id);
        }

   
      } else if (frame->request == KHubCtrlRequestRegisterAPService) {
        // RegisterAP received. Introduce a few variables to make
        // code more readable (Compiler should optimize these out)
        const unsigned int bsize = sizeof(THubCtrlProtocolHeader);
        const unsigned int rsize = sizeof(THubCtrlSubHeaderRegisterAPService);
        const unsigned int asize = sizeof(THubCtrlSubHeaderAuthentication);
        
        printw("\rReceived a reply for RegisterAPService Id=%u, operation %s\n",
               frame->message_id,
               frame->result==KHubCtrlResultOK ? "was a success!" :
               frame->result==KHubCtrlResultIdReserved ? "failed due to reserved ID." :
               frame->result==KHubCtrlResultAuthExpired ? "failed due to expired authorization." :
               "produced an unknown error!");
      
        if (size == bsize+rsize+asize) {
          const THubCtrlSubHeaderRegisterAP *rap = (void*)(data+bsize); 
          const THubCtrlSubHeaderAuthentication *auth = (void*)(data+bsize+rsize); 
        
          printw("RegisterAPService Reply Id=%u contained request for NodeID: 0x%08x\n",
                 frame->message_id, rap->peer_id);
        
          const int a = authenticateMessage(data, size, auth, (void*)secret_key, secret_key_size, 10);
          if (a == 0) {
            printw("RegisterAPService Reply Id=%u was succesfully Authenticated\n",frame->message_id);
 
          } else if (a == 1) {
            printw("RegisterAPService Reply Id=%u, received reply had expired!\n", frame->message_id);

          } else {
            printw("RegisterAPService Reply Id=%u failed to Authenticate!!!\n", frame->message_id);

          }



        } else if (size == bsize+rsize) {
          const THubCtrlSubHeaderRegisterAP *rap = (void*)(data+bsize); 
          printw("RegisterAPService Reply Id=%u contained request for NodeID: 0x%08x\n",
                 frame->message_id, rap->peer_id);

        } else if (size == bsize) {
          printw("RegisterAPService Reply Id=%u contained no additional headers.\n",

                 frame->message_id);
        }

   
      } else if (frame->request == KHubCtrlRequestServerEcho &&
                 size >= sizeof(THubCtrlSubHeaderTimestamp)) {
        const THubCtrlSubHeaderTimestamp *ts = (void*)(frame+1);
        long ss,sus;
        ownTime_GetTimeOfDay(&ss,&sus);
        const uint64_t received = (ss*1000000) + sus;
        const uint64_t sent = ((uint64_t)ts->seconds*1000000) + ((uint64_t)ts->useconds);
        const uint64_t rtt_time = received - sent;
        printw("\rReceived ServerEcho Id=%u, RoundTripTime was %llu us\n", 
               frame->message_id, rtt_time);
      
      } else {
        printw("\rUnhandled HubCtrl reply with requestId = 0x%02x, resultCode = 0x%02x\n",
               frame->request,
               frame->result); 

      }

      notifyReply(frame->message_id);

    } else {
      // Not a reply.
      if (frame->request == KHubCtrlRequestClientEcho &&
          size == sizeof(THubCtrlProtocolHeader)+sizeof(THubCtrlSubHeaderTimestamp)) {
        
        // Send AS-IS back to AP.
        protocolDispSendPacket(&aplink,(void*)frame, size);

        // Sent
        dPrint(ODTEST,"Replied to ClientEcho!");
        
      }
    }
  } else {
    dPrint(ODWARN,"Warning: Received invalid HubCtrlProtocol frame!");
    
  }
  /*    } else if (frame->request == KHubCtrlRequestClientEcho) {
        printw("QUCK! We should reply to Client Echo! (Unimplemented :/)\n");*/
  
  
}
//*****************************************************************************

static void requestOwnId(TProtocolDisp *link)
{
  THubCtrlProtocolHeader frame;

  // Clear header.
  memset(&frame, 0x00, sizeof(frame));
  
  // Assign identification information
  protocolIdentificationConstructHeader(&frame.protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        sizeof(THubCtrlProtocolHeader));
  
  // Copy parameters
  frame.request = KHubCtrlRequestQueryLocalNodeId;
  frame.flags = 0x00;
  frame.subheader_count = 0;
  frame.result = 0;
  frame.message_id = ++messageSeq;
  
  protocolDispSendPacket(link,(void*)&frame,sizeof(frame));
}
//*****************************************************************************

static void showwait()
{
  const char value[] = "|/-\\";
  static int phase = 0;
  printw("\r %c: ", value[phase]);
  phase = (phase + 1) % 4;
}
//*****************************************************************************

static void test_QueryLocalNodeId(void)
{


  THubCtrlProtocolHeader frame;

  printw("Query Local NodeID:\n");
  
  const double begin = ownTime_get_s();
  // Assign identification information
  protocolIdentificationConstructHeader(&frame.protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        sizeof(THubCtrlProtocolHeader));
  
  // Copy parameters
  frame.request = KHubCtrlRequestQueryLocalNodeId;
  frame.flags = 0x00;
  frame.subheader_count = 0;
  frame.result = 0;
  frame.message_id = ++messageSeq;
  
  protocolDispSendPacket(&aplink,(void*)&frame, sizeof(frame));
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));
}
//*****************************************************************************

static void test_Version(void)
{
  THubCtrlProtocolHeader frame;
  
  printw("Query HubVersion:\n");
  
  const double begin = ownTime_get_s();
  // Assign identification information
  protocolIdentificationConstructHeader(&frame.protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        sizeof(THubCtrlProtocolHeader));
  
  // Copy parameters
  frame.request = KHubCtrlRequestHubVersion;
  frame.flags = 0x00;
  frame.subheader_count = 0;
  frame.result = 0;
  frame.message_id = ++messageSeq;
  
  protocolDispSendPacket(&aplink,(void*)&frame, sizeof(frame));
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));
}
//*****************************************************************************

static void test_RegisterAP(void)
{
  unsigned int timestamp = time(NULL);
  printw("Testing Register AccessPoint (RegisterAP):\n");
  printw("Secret? ['%s']: ", secret_key);
  scanw("%s", secret_key);
  secret_key_size = strlen(secret_key);
  
  printw("Requested HubID? [0x%08lx]: ", requested_hubId);
  scanw("%lx", &requested_hubId);


  printw("Timestamp? [now=%lu]: ", timestamp);
  scanw("%lu", &timestamp);
  halfdelay(1);




  const unsigned int totsize = ( sizeof(THubCtrlProtocolHeader) + 
                                 sizeof(THubCtrlSubHeaderRegisterAP) + 
                                 sizeof(THubCtrlSubHeaderAuthentication) );
  uint8_t buffer[totsize];
  THubCtrlProtocolHeader *frame = (THubCtrlProtocolHeader *)buffer;
  THubCtrlSubHeaderRegisterAP *rap = (THubCtrlSubHeaderRegisterAP *)(frame+1);
  THubCtrlSubHeaderAuthentication *auth = (THubCtrlSubHeaderAuthentication *)(rap+1);

  
  const double begin = ownTime_get_s();
  // Assign identification information
  protocolIdentificationConstructHeader(&frame->protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        totsize);
  
  // Assign base parameters
  frame->request = KHubCtrlRequestRegisterAP;
  frame->flags = 0x00;
  frame->subheader_count = 2;
  frame->result = 0;
  frame->message_id = ++messageSeq;
  

  // Assign Request parameter
  rap->subtype = KHubCtrlSubtypeRegisterAP;
  rap->subsize = sizeof(THubCtrlSubHeaderRegisterAP);
  rap->peer_id = requested_hubId;

  // Assign authentication data
  auth->subtype = KHubCtrlSubtypeAuthentication;
  auth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
  memset(auth->sha1, 0x00, sizeof(auth->sha1));
  auth->timestamp_s = timestamp;
         
  // Now; calculate authentication data SHA-1; via separate buffer.
  uint8_t sha1[20];
  unsigned int sha1_size = 20;
  apsecurityCalculateHMAC(sha1,
                          &sha1_size,
                          (uint8_t*)secret_key,
                          secret_key_size,
                          buffer,
                          totsize);
  assert(sha1_size == 20);
  memcpy(auth->sha1, sha1, sizeof(sha1));


  // Complete. SEND!
  protocolDispSendPacket(&aplink,(void*)frame, totsize);
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));

}
//*****************************************************************************

static void test_ServerEcho()
{
  printw("ServerEcho:\n");
  
  const double begin = ownTime_get_s();

  const unsigned int msgsize = sizeof(THubCtrlProtocolHeader) + sizeof(THubCtrlSubHeaderTimestamp);
  uint8_t buffer[msgsize];
  THubCtrlProtocolHeader *frame = (void*)buffer;  
  
  // Assign identification information
  protocolIdentificationConstructHeader(&frame->protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        msgsize);
  
  // Copy parameters
  frame->request = KHubCtrlRequestServerEcho;
  frame->flags = 0x00;
  frame->subheader_count = 1;
  frame->result = 0;
  frame->message_id = ++messageSeq;

  THubCtrlSubHeaderTimestamp *ts = (void*)(frame+1);
  ts->subtype = KHubCtrlSubtypeTimestamp;
  ts->subsize = sizeof(THubCtrlSubHeaderTimestamp);
  long s,us;
  ownTime_GetTimeOfDay(&s, &us);
  ts->seconds = s;
  ts->useconds = us;

  protocolDispSendPacket(&aplink,(void*)buffer, msgsize);
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));

}
//*****************************************************************************

static void test_RegisterAPService()
{
  unsigned int timestamp = time(NULL);
  printw("Testing Register AP Service (RegisterAPService):\n");
  printw("Secret? ['%s']: ", secret_key);
  scanw("%s", secret_key);
  secret_key_size = strlen(secret_key);
  
  printw("Requested HubID? [0x%08lx]: ", requested_hubId);
  scanw("%lx", &requested_hubId);


  printw("Timestamp? [now=%lu]: ", timestamp);
  scanw("%lu", &timestamp);
  halfdelay(1);

  const unsigned int totsize = ( sizeof(THubCtrlProtocolHeader) + 
                                 sizeof(THubCtrlSubHeaderRegisterAPService) + 
                                 sizeof(THubCtrlSubHeaderAuthentication) );
  uint8_t buffer[totsize];
  THubCtrlProtocolHeader *frame = (THubCtrlProtocolHeader *)buffer;
  THubCtrlSubHeaderRegisterAPService *rap = (THubCtrlSubHeaderRegisterAPService *)(frame+1);
  THubCtrlSubHeaderAuthentication *auth = (THubCtrlSubHeaderAuthentication *)(rap+1);

  
  const double begin = ownTime_get_s();
  // Assign identification information
  protocolIdentificationConstructHeader(&frame->protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        totsize);
  
  // Assign base parameters
  frame->request = KHubCtrlRequestRegisterAPService;
  frame->flags = 0x00;
  frame->subheader_count = 2;
  frame->result = 0;
  frame->message_id = ++messageSeq;
  

  // Assign Request parameter
  rap->subtype = KHubCtrlSubtypeRegisterAPService;
  rap->subsize = sizeof(THubCtrlSubHeaderRegisterAPService);
  rap->requested_id = requested_hubId;

  // Assign authentication data
  auth->subtype = KHubCtrlSubtypeAuthentication;
  auth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
  memset(auth->sha1, 0x00, sizeof(auth->sha1));
  auth->timestamp_s = timestamp;
         
  // Now; calculate authentication data SHA-1; via separate buffer.
  uint8_t sha1[20];
  unsigned int sha1_size = 20;
  apsecurityCalculateHMAC(sha1,
                          &sha1_size,
                          (uint8_t*)secret_key,
                          secret_key_size,
                          buffer,
                          totsize);
  assert(sha1_size == 20);
  memcpy(auth->sha1, sha1, sizeof(sha1));


  // Complete. SEND!
  protocolDispSendPacket(&aplink,(void*)frame, totsize);
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));
  
}
//*****************************************************************************

static void test_RegisterAPClient()
{
  unsigned int timestamp = time(NULL);
  printw("Testing Register AP Client (RegisterAPClient):\n");
  printw("Secret? ['%s']: ", secret_key);
  scanw("%s", secret_key);
  secret_key_size = strlen(secret_key);

  printw("Timestamp? [now=%lu]: ", timestamp);
  scanw("%lu", &timestamp);
  halfdelay(1);

  const unsigned int totsize = ( sizeof(THubCtrlProtocolHeader) + 
                                 sizeof(THubCtrlSubHeaderAuthentication) );
  uint8_t buffer[totsize];
  THubCtrlProtocolHeader *frame = (THubCtrlProtocolHeader *)buffer;
  THubCtrlSubHeaderAuthentication *auth = (THubCtrlSubHeaderAuthentication *)(frame+1);

  
  const double begin = ownTime_get_s();
  // Assign identification information
  protocolIdentificationConstructHeader(&frame->protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        totsize);
  
  // Assign base parameters
  frame->request = KHubCtrlRequestRegisterAPService;
  frame->flags = 0x00;
  frame->subheader_count = 1;
  frame->result = 0;
  frame->message_id = ++messageSeq;
  
  // Assign authentication data
  auth->subtype = KHubCtrlSubtypeAuthentication;
  auth->subsize = sizeof(THubCtrlSubHeaderAuthentication);
  memset(auth->sha1, 0x00, sizeof(auth->sha1));
  auth->timestamp_s = timestamp;
         
  // Now; calculate authentication data SHA-1; via separate buffer.
  uint8_t sha1[20];
  unsigned int sha1_size = 20;
  apsecurityCalculateHMAC(sha1,
                          &sha1_size,
                          (uint8_t*)secret_key,
                          secret_key_size,
                          buffer,
                          totsize);
  assert(sha1_size == 20);
  memcpy(auth->sha1, sha1, sizeof(sha1));


  // Complete. SEND!
  protocolDispSendPacket(&aplink,(void*)frame, totsize);
  
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for QueryId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     

  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));
  
}
//*****************************************************************************

static int commandHandler(int value)
{
  int r = 1;
  printw("\n");
  switch(value) {
  case 'l':
    test_QueryLocalNodeId();
    break;

  case 'v':
    test_Version();
    break;

  case 'R':
    test_RegisterAP();
    break;

  case 'e':
    test_ServerEcho();
    break;

  case 's':
    test_RegisterAPService();
    break;

  case 'c':
    test_RegisterAPClient();
    break;


  case 'q':
    printw("Terminate program...\n");
    r = 0;
    break;

  default:
  case 'h':
    printw("\nAvailable Tests:\n");
    printw(" 'l'       Query local NodeID\n");
    printw(" 'v'       Request remote Access Point version\n");
    printw(" 'R'       Test Registering an AccessPoint (Act as one)\n");   
    printw(" 's'       Test Registering an AP Service (Act as one)\n");
    printw(" 'c'       Test Registering an AP Client  (Act as one)\n");
    printw(" 'e'       Server Echo\n");

    printw(" 'q'       Quit\n");
    printw("\n");
    break;
  }
  return r;
}
//*****************************************************************************

static int ExecuteTest(TProtocolDisp *link)
{
  int result = 0;

  // Init sync mechanisms
  lock = ownMutex_Init();
  cond = ownCond_Init();
  
  
  // Register callback,
  result = protocolDispRegisterCallback(link, HUBCTRLPROTOCOL_NUMBER, 
                                        hubctrlProtocolCallback, NULL);
  assert(result == 0);


  // Init curses
  w = initscr(); 
  cbreak(); 
  //  noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  //nodelay(w, 1);
  halfdelay(1);
  atexit( (void (*)(void))endwin );

  int value;

  // Start processor
  protocolDispProcessorThreadHandle = ownThread_Create((void*)protocolDispProcessorThread, link);

  while (run) {
    showwait();
    
    // Read input
    value = getch();
    
    // Handle input
    if (value != ERR) {
      clear();
      refresh();

      run = commandHandler(value);
    }
  }
  return result;
}
//*****************************************************************************

static void showUsage()
{
}
//*****************************************************************************

static int SignalHandlerThread(void *t)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(ODSTATUS,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      run = 0;
      ownThread_Signal(signalHandlerThread, SIGINT);
      ownSleep_ms(1000);
      break;
      
    default:
      dPrint(ODSTATUS,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  int paramParseLoop = 1;
  //  unsigned int service_address = 0x00000001;
  //  const char *authenticationFile = NULL;
  char targetAddress[255];
  int targetTcpPort = 0; // Port to use
  int r = 0;

  
  // Set up utils (debugs mostly)
  debugInit();
  debugSetGlobalDebugLvl(ODINFO);
  debugSetLogFilename("nameservice_test.log");
  debugRemoveMode( DEBUG_MODE_TIMESTAMP_HH_MM_SS | DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS );
  debugAddMode( DEBUG_MODE_TIMESTAMP_DELTA_SS_US );

  // Now; Set to BLOCK ALL SIGNALS
  InitSignalHandler();
  
  // Print version
  printf("\nGIMnet HubCtrl protocol TestApplication - %s\n\n", GIMNETAP_HUBCTRLPROTOCOL_TESTAPP_VERSIONSTRING);
  
  
  // Decode command line parameters.
  while(paramParseLoop) {
    int c;
    c = getopt(argc, argv, "u:p:qvh");
    switch(c) {
      
    case 'p':
      // Port to use
      targetTcpPort = atoi(optarg);
      break;
      
    case 'u':
      // Host to
      strncpy(targetAddress, optarg, sizeof(targetAddress)-1);
      targetAddress[sizeof(targetAddress)-1] = '\0';
      break;
      
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////

    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0) verbose++;
      debugSetGlobalDebugLvl(verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(verbose);
      debugSetLogFilename(NULL);
      break;
      
    case 'h':
    case '?':
      showUsage();
    exit(1);
    break;
    
    case -1:
      paramParseLoop = 0;
      break;
    }
  }

  // Now; got all parameters -> start processing
  // 1. Initialize TCP link
  int v = 0;
  v = protocolDispInit(&aplink);
  dPrint(ODTEST,"Connecting to '%s' at port %u", targetAddress, targetTcpPort);
  v &= protocolDispConnect(&aplink, targetAddress, targetTcpPort);
  if (v == PROTOCOLDISPATCHER_OK) {
    // OK!
    r = ExecuteTest(&aplink);

  } else {
    // FAILED!
  } 

  printf("tcpHub exiting.\r\n");

  return r;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
